
#include "kinematics/kinematics.h" /* decls for kinematicsForward, etc. */
#include <string.h>
#include "rtapi/rtapi_math.h"
#include "rtapi/rtapi.h"     /* RTAPI realtime OS API */
#include "rtapi/rtapi_app.h" /* RTAPI realtime module decls */
#include "hal/hal.h"
#include "posemath.h"
#include "time.h"
#undef theta

int getToolMovPos(const ZucPose* world, const ZucPose* pos, ZucPose* out)
{
    PmHomogeneous t_world, t_pos, t_out;
    PmRpy rpy;
    t_world.tran = world->tran;
    t_pos.tran = pos->tran;
    rpy.y = world->a * PM_PI / 180.0;
    rpy.p = world->b * PM_PI / 180.0;
    rpy.r = world->c * PM_PI / 180.0;
    pmRpyMatConvert(&rpy, &t_world.rot);

    rpy.y = pos->a * PM_PI / 180.0;
    rpy.p = pos->b * PM_PI / 180.0;
    rpy.r = pos->c * PM_PI / 180.0;
    pmRpyMatConvert(&rpy, &t_pos.rot);

    pmHomHomMult(&t_world, &t_pos, &t_out);
    out->tran = t_out.tran;
    pmMatRpyConvert(&t_out.rot, &rpy);
    out->a = rpy.y * 180.0 / PM_PI;
    out->b = rpy.p * 180.0 / PM_PI;
    out->c = rpy.r * 180.0 / PM_PI;

    return 0;
}
